#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"


class static_broadcaster :public rclcpp::Node
{
private:
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf2_;
    // tf2_ros::StaticTransformBroadcaster static_tf2_;
public:
    static_broadcaster(/* args */);
    ~static_broadcaster();
};

static_broadcaster::static_broadcaster(/* args */):Node("static_broadcaster_node")
{
    static_tf2_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = "child";

    t.transform.translation.x = 10;
    t.transform.translation.y = 20;
    t.transform.translation.z = 30;
    tf2::Quaternion q;
    q.setRPY(40,50,60);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    static_tf2_->sendTransform(t);
    // static_tf2_.sendTransform(t);
}

static_broadcaster::~static_broadcaster()
{
}


int main(int argc, char *argv[])
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<static_broadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
